
module Mathmodule

#inputs a degree in and return the value of it's radian
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end

def dist(x1,y1,x2,y2)
  inner=((x2-x1)**2)+((y2-y1)**2)
  distance=movedis=Math.sqrt(inner )
return distance
end


def math_bayesupdate(oldprob,sensorprob)
  #uses the inverse model sensor for upgrading the bayes value
  # newprob= 1- inverse(1+ (P(S)/1-P(S))*P(old)/(1-P(old))
  probsen=sensorprob
  inverse=1.0+( (probsen/(1.0-probsen)) * (oldprob/(1.0-oldprob))  )
  inverse=1.0/inverse
  prob=1-inverse  
 return prob  
end

def math_standarddist(wallpos,maxlaser,laserscale,distri)
 #initiation values
  pi=3.1415
  sonarmodel=Array.new() #the sonar model to keep the data   
  peak=0.7
  totalscan= maxlaser*laserscale
#normal distribution parameters
  # distri=0.1
  sq=Math.sqrt(distri) 
  constpara=1.0/(Math.sqrt(2*pi)*sq)   
#find max
  range=(wallpos)
  expcons=(-(range-wallpos)**2.0)/(2*(distri))
  max=constpara*Math.exp(expcons) 
  
#calculate each one based on a normal distribution
  ((totalscan+(distri*totalscan).to_i)).to_i.times{|i|
  #(totalscan).to_i.times{|i|
  range=(i-1.0)/laserscale
  expcons=(-(range-wallpos)**2.0)/(2*(distri))
  yvalue=constpara*Math.exp(expcons) 
  #normalize
  yvalue=(yvalue/max)*peak
  if(range<wallpos)
      if(yvalue>0.5 )
        sonarmodel[i]=(yvalue)
      else
        sonarmodel[i]=0.40
      end
  else
      if(yvalue>0.5 )
        sonarmodel[i]=(yvalue)
      else
        sonarmodel[i]=0.0
      end
      
    end
 
      
  }
 
  return sonarmodel
end
  
end